/*
Clarke and Park forward and inverse transforms and the relevant
supporting code that goes along with that whole thing.
*/

#include <p30F4011.h>
#include "settings.h"
#include "AC-Controller.h"
#include "globals.h"
#include "acim_foc.h"
#include "eeprom.h"


// Field Oriented Control variables ///////////////////////////////////////////////
volatile int i_b;				// The A/D current value for sensor 2.  in [-128, 128].  You only need 2 of them, because i_a + i_b + i_c = 0,
								// but I use i_alpha instead of i_a, and i_c isn't used.
volatile int i_alpha, i_beta;	// Result of Clarke Transform.  in [-128, 128] or so.
volatile long int temp_i_alpha, temp_i_b; 	// temp variables.
volatile int i_d, i_q;			// Result of the Park Transform.  in [-128, 128] or so.
volatile unsigned int tempOffset1, tempOffset2; // in [0, 511].  Offsets for the sin lookup table.

volatile long int cos_theta, sin_theta;  // in [-(2^15), 2^15].  They are only used in long multiplications, so I was just trying to avoid having to cast them as long.
volatile long int v_d, v_q;		// These are the "duties" that I set based on the throttle position.  in [-(2^16)*1023, (2^16)*1023].
volatile long int v_d_small, v_q_small; // auxiliary variables to avoid repeating the same operation multiple times.
volatile int v_alpha, v_beta;	// Result of doing the inverse Park transform.  in [-1023, 1023].  
volatile long int temp_v_beta;
volatile int Vr1, Vr2, Vr3;  	// Result of doing the modified inverse Clarke transform.  in [-1023, 1023].


volatile unsigned int T1, T2;		// The times for how to share the duty between adjacent transistor switching states.  in [0, MAX_PWM/2].  T1 + T2 = MAX_PWM.
volatile unsigned int Ta, Tb, Tc;  	// The 3 duties.  In [0, MAX_PWM].

volatile int rotorFluxAngle; 		// This is the rotor flux angle. In [0, 1023], but may be negative during part of the calculation below.
volatile unsigned int tickCount;    // Used as a temporary variable to hold POSCNT.  in [0, 1024].
volatile unsigned int rotorPos = 0; // start it out as 0, then figure out what it really is.  It will be a constant for this motor forever.  in [0, 1023].  It is how far past the index does the 
									// motor stop spinning when i_q = 0, and i_d is throttle.  Make sure you pass index to reset POSCNT.  Then hit the throttle.  When the post stops spinning,
									// you have found rotorPos.  i_q will have maximum affect here, since i_q is at 90 degrees to i_d.  i_d does nothing, so i_q will be perfect.
unsigned int numberOfPolePairs; //be sure to get the value from EEPROM
long mechanicalSpeed;
/////////////////////////////////////////////////////////////////

void Motor_FOC_Init() {
	numberOfPolePairs = ee_get_motor_poles();
}


void MotorLoop_FOC() {
		/*this should be explained... i_alpha and i_b are directly input from the ADC. They are connected
		//to the two current sensors. The sensors are bidirectional and should be sitting at (current_bias) or
		//about 512 when no current is flowing. So, if they are way over or under this either the current sensor
		//is wacked/missing or the motor is currently melting into a puddle on your floor
		*/
		if (i_alpha < 100 || i_alpha > 900 || i_b < 100 || i_b > 900)  // || i_c < 100 || i_c > 900) 
			fault_bits |= VREF_FAULT;
		
		i_alpha -= current_bias;  	// unsigned int.  subtracting 512 (vref) puts it in the range [-128, 128], assuming the max current is limited to 'x' in the LEM Hass 'x'-s.
									// For example, the LEM Hass 50-s has an output of 128 + vref if the current is 50 amps, where vref is approximately 512 = 2.5v
		i_b -= current_bias;
		//temp_i_alpha = ((long)i_alpha) << 16;
		//temp_i_b = ((long)(i_b << 1))*37837L; 
		// clarke transform.
		// i_beta = (i_alpha + 2*i_b)/sqrt(3)
		i_beta = (int)((((long)(i_alpha + i_b + i_b))*37837L) >> 16);  // i_alpha, i_b are in [-128, 128].  37837 is (1/sqrt(3)) * 2^16

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// How often should I check the rotor flux angle?  Should I try the PI loop at 4kHz?
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		/*tickCount = POSCNT;	// Get the number of ticks since the last index pulse.
		numTicks = tickCount - oldTickCount;
		oldTickCount = tickCount;
		if (numTicks < 0) // NUM_TICKS = 1024, since the encoder has 512 ticks, and I have 2 pulses per encoder tick.
			numTicks += 1024; */
		//mechanicalSpeed = ((long)(numTicks))*249984L;  // revolutions per second = rev/1024Ticks * numTicks/pwmPeriod * pwmPeriod/0.000128Seconds * 65536
		mechanicalSpeed = 0; //obviously fix this by uncommenting the above code!
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute Rotor Flux Angle 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		ComputeRotorFluxAngle();
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// park transform.
		// rotorFluxAngle is in [0, 1023].
		// sin(theta + 90 degrees) = cos(theta).
		// I want the 2 angles to be in [0, 511] so I can use the lookup table.
		tempOffset1 = ((rotorFluxAngle + 256) & 1023) >> 1;  	// This assumes NUM_TICKS IS 1023. To advance 90 degrees on a scale of 0 to 1023, you add 256, 
																// and then do "& 1023" to make it wrap around if overflow occurred.  Then you divide by 2 to put it in the range [0, 511].
		tempOffset2 = rotorFluxAngle >> 1;  // THIS ASSUMES NUM_TICKS IS 1023.  so both offsets will be in [0, 511]. 
		cos_theta = (long)_sin[tempOffset1];  // 
		sin_theta = (long)_sin[tempOffset2];  // 
		i_d = (int)((((long)i_alpha)*cos_theta + ((long)i_beta)*sin_theta) >> 15); 	// you want i_d to be 0 in the case of PMSM.  This is the measured i_d.  
																					// Reference i_d will be 0.  You will use a PI loop or something to make measured i_d CHASE 0.
		i_q = (int)((-((long)i_alpha)*sin_theta + ((long)i_beta)*cos_theta) >> 15); // i_q is the torque component, so you want throttle proportional to torque, so you want i_q 
																					// to chase throttle position.

		// Now do the PI loop, so as to cause i_d to get closer to 0, and to get i_q closer to throttle position, in the case of the PMSM controller.
		// To avoid tuning a PI loop at the moment, I'm hoping that this way of incrementing v_d and v_q is good enough just to get it to spin, and then
		// I can change it to a PI loop.
		// If you want behavior just like a brushed DC controller, set i_d to i_q. 

		// THIS IS REVERSED so that I can figure out the offset.  Just for testing at startup!!!  Must find rotor offset from the index!
		if (i_q < throttle_feedback) {
			v_q += 32767L;	// You have to go 1500 of these increments to go from v_q = 0 to v_q = max.  So you should get finer control.
		}
		else if (i_q > throttle_feedback) {
			v_q -= 32767L;
		}
		if (i_d < 0) {
			v_d += 32767L;
		}
		else if (i_d > 0) {
			v_d -= 32767L;
		}
		// clamp v_d and v_q
		if (v_d < -67043328L)	// -1023*(2^16)
			v_d = -67043328L;
		else if (v_d > 67043328L) // 1023*(2^16)
			v_d = 67043328L;
		
		if (v_q < -67043328L)
			v_q = -67043328L;
		else if (v_q > 67043328L)
			v_q = 67043328L;
	
		v_d_small = (v_d >> 16);	// in [-1023, 1023]
		v_q_small = (v_q >> 16);
		// Now do inverse Park transform.		
		v_alpha = (int)((v_d_small*cos_theta - v_q_small*sin_theta) >> 15);	// shift right 15 because sin and cos have been shifted left by 15.
		v_beta =  (int)((v_d_small*sin_theta + v_q_small*cos_theta) >> 15);	//
		// v_alpha and v_beta are now in the range [-1023, 1023].

		// Now do the "modified" inverse Clarke transform.
		//  Vr1 = Vbeta
        //	Vr2 = (-Vbeta/2 + sqrt(3)/2 * Valpha)
        //  Vr3 = (-Vbeta/2 - sqrt(3)/2 * Valpha)

		// V_alpha and V_beta are in the range [-1023, 1023].  In the worst case, Vr2 and Vr3 can be in the range [-1.366*1023, 1.366*1023] = [-1397, 1397].
		// By the way, 1.366 = 1/2 + sqrt(3)/2.  
		// However, Vr1 = v_beta, and the biggest v_beta can be is 1023!!! But you want Vr1, Vr2, and Vr3 to all be in [-1023, 1023].
		// So, you need to clamp Vr2 and Vr3 so they will be in [-1023, 1023].
		Vr1 = v_beta;
		temp_v_beta = ((long)v_beta) << 16;
		// 113512 = sqrt(3)*(2^16).
		Vr2 =  (int)((113512L*((long)v_alpha) - temp_v_beta) >> 17); // you have to divide by 2 at the end.  So shift by 16, and 1 more all at once.
		Vr3 = -(int)((113512L*((long)v_alpha) + temp_v_beta) >> 17);
		if (Vr2 > 1023)
			Vr2 = 1023;
		else if (Vr2 < -1023)
			Vr2 = -1023;
		if (Vr3 > 1023)
			Vr3 = 1023;
		else if (Vr3 < -1023)
			Vr3 = -1023;

		CalcRefVec();

}

void CalcTimes(void) {
    Tc = (MAX_PWM - T1 - T2) >> 1;
    Tb = Tc + T1;
    Ta = Tb + T2;
}

// 
// Vr1, Vr2, and Vr3 are in the range [-1023, 1023].
// Vr1, Vr2, and Vr3 are the result of doing the "modified" inverse clarke transformation, where v_alpha and v_beta are swapped.
void CalcRefVec( void ) { 
	if( Vr1 >= 0 ) {       
        // (xx1)
        if( Vr2 >= 0 ) {
            // (x11)
            // Must be Sector 3 since Sector 7 not allowed
            // Sector 3: (0,1,1)  0-60 degrees
            T1 = Vr1; // Now we know that Vr2 and Vr1 is in [0, 1023].
            T2 = Vr2; // This is what the assembly says, but not the C.  Friggen retarded crap.  And I don't understand what they are doing either.  Horrible.
            CalcTimes();
            PDC1 = Ta;
            PDC2 = Tb;
            PDC3 = Tc;
        }
        else {            
            // (x01)
            if( Vr3 >= 0 ) {
                // Sector 5: (1,0,1)  120-180 degrees
                T2 = Vr1;
                T1 = Vr3;
                CalcTimes();
                PDC1 = Tc;
                PDC2 = Ta;
                PDC3 = Tb;
			}
            else {
                // Sector 1: (0,0,1)  60-120 degrees
                T2 = -Vr2;
                T1 = -Vr3;
                CalcTimes();
                PDC1 = Tb;
                PDC2 = Ta;
                PDC3 = Tc;
            }
        }
 	}       
    else {
        // (xx0)
        if( Vr2 >= 0 ) {
            // (x10)
            if( Vr3 >= 0 ) {
                // Sector 6: (1,1,0)  240-300 degrees
                T2 = Vr3;
                T1 = Vr2;
                CalcTimes();
                PDC1 = Tb;
                PDC2 = Tc;
                PDC3 = Ta;
            }
            else {
                // Sector 2: (0,1,0)  300-0 degrees
                T2 = -Vr3;
                T1 = -Vr1;
                CalcTimes();
                PDC1 = Ta;
                PDC2 = Tc;
                PDC3 = Tb;
            }
        }
        else {            
            // (x00)
            // Must be Sector 4 since Sector 0 not allowed
            // Sector 4: (1,0,0)  180-240 degrees
            T2 = -Vr1;
            T1 = -Vr2;
            CalcTimes();
            PDC1 = Tc;
            PDC2 = Tb;
            PDC3 = Ta;
        }
    }
}

void ComputeRotorFluxAngle() {

	long magnetizingCurrent, slipSpeed, rotorFluxSpeed, rotorFluxAngle;
//;	 Physical form of equations:
//;  Magnetizing current (amps):
//;     Imag = Imag + (fLoopPeriod/fRotorTmConst)*(Id - Imag)
//;
//;  Slip speed in RPS:
//;     VelSlipRPS = (1/fRotorTmConst) * Iq/Imag / (2*pi)
//;
//;  Rotor flux speed in RPS:
//;     VelFluxRPS = iPoles * VelMechRPS + VelSlipRPS
//;
//;  Rotor flux angle (radians):
//;     AngFlux = AngFlux + fLoopPeriod * 2 * pi * VelFluxRPS

	// Compute Magnetizing current:
	// 108 = loopPeriodInSecond/rotorTimeConstant * 65536, where rotorTimeConstant = 0.078 and loopPeriod is 128us
	// magnetizingCurrent is in [-128, 128].  The same range as i_d
	magnetizingCurrent = ((magnetizingCurrent << 16) + (108L)*(i_d - magnetizingCurrent)) >> 16;
	
	// Compute the slip speed in revolutions per second, but scaled by 65536: 128*65536 would mean 7680 rpm.
	slipSpeed = (133717L * ((long)i_q)) / magnetizingCurrent;
	
	// rotor Flux Speed in revolutions per second, but scaled by 65536: 128*65536 would mean 7680 rpm.
	// mechanical speed is in revolutions per second, but scaled by 65536.
	rotorFluxSpeed = numberOfPolePairs*mechanicalSpeed + slipSpeed;

	// rotor flux angle is in radians, but scaled by 65536.
	rotorFluxAngle = rotorFluxAngle + ((53L*rotorFluxSpeed) >> 16);
}

